#ifndef __BNO055_H
#define __BNO055_H



//#define M_PI 3.14159265358979323846
//#define DEG_TO_RAD (PI / 180.0)
//#define RAD_TO_DEG (180.0 / PI)

// BNO055常量定义
#define BNO055_I2C_ADDR         (0x28 << 1)  // 默认I2C地址
#define BNO055_CHIP_ID          0xA0         // 期望的芯片ID

// 寄存器地址
#define BNO055_REG_CHIP_ID      0x00
#define BNO055_REG_ACCEL_DATA   0x08
#define BNO055_REG_EULER_DATA   0x1A
#define BNO055_REG_QUAT_DATA    0x20
#define BNO055_REG_OPR_MODE     0x3D
#define BNO055_REG_PWR_MODE     0x3E
#define BNO055_REG_CALIB_STAT   0x35

// 操作模式定义
typedef enum {
    CONFIG_MODE = 0x00,
    NDOF_MODE = 0x0C,
	IMU_MODE = 0x08
} BNO055_OpMode_t;

// 电源模式定义
typedef enum {
    POWER_MODE_NORMAL = 0x00
} BNO055_PowerMode_t;

// 传感器数据结构
typedef struct {
    int16_t AccX, AccY, AccZ;
    int16_t GyroX, GyroY, GyroZ;
    int16_t quat1, quat2, quat3, quat4;
	float gyro1,gyro2,gyro3;
} BNO055_Data_t;

// 校准状态结构
typedef struct {
    uint8_t system;
    uint8_t gyro;
    uint8_t accel;
    uint8_t mag;
} BNO055_CalibStatus_t;

// 函数声明
uint8_t BNO055_OptimizedInit(I2C_HandleTypeDef *hi2c);
uint8_t BNO055_Init(I2C_HandleTypeDef *hi2c);
void BNO055_loop(void);
uint8_t BNO055_ReadIMUForFASTLIO(I2C_HandleTypeDef *hi2c, float gyro[3], float accel[3]);
uint8_t BNO055_DebugReadIMUForFASTLIO(I2C_HandleTypeDef *hi2c, float gyro[3], float accel[3]);
void BNO055_Diagnostic(I2C_HandleTypeDef *hi2c);
void CheckCalibrationStatus(void);
uint8_t BNO055_ReadCalibStatus(I2C_HandleTypeDef *hi2c, BNO055_CalibStatus_t *calib);
uint8_t BNO055_ReadAccel(I2C_HandleTypeDef *hi2c, BNO055_Data_t *data);
uint8_t BNO055_ReadEuler(I2C_HandleTypeDef *hi2c, BNO055_Data_t *data);
uint8_t BNO055_ReadQuaternion(I2C_HandleTypeDef *hi2c, BNO055_Data_t *data);
void BNO055_DebugReadGyro(I2C_HandleTypeDef *hi2c);
void BNO055_DebugReadAccel(I2C_HandleTypeDef *hi2c);
void BNO055_DebugReadEuler(I2C_HandleTypeDef *hi2c);
void BNO055_DebugReadQuaternion(I2C_HandleTypeDef *hi2c);
void BNO055_DebugReadLinearAccel(I2C_HandleTypeDef *hi2c);
void BNO055_DebugReadGravity(I2C_HandleTypeDef *hi2c);
void BNO055_CalibrateAccelerometer(I2C_HandleTypeDef *hi2c);
void VerifyFASTLIODataQuality(void);


uint8_t BNO055_ReadIMUForFASTLIO1(I2C_HandleTypeDef *hi2c, float gyro[3], float accel[3]);
// 全局变量声明
extern BNO055_Data_t BNO055_Data;

#endif
